{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# 标定头部的两个相机之间的关系"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "%pylab inline\n",
    "from sympy import init_printing,Matrix\n",
    "init_printing(use_latex='mathjax')\n",
    "from scipy import weave\n",
    "import os\n",
    "import cv2\n",
    "import sql\n",
    "import pickle\n",
    "from robot import r2t,t2r,transl,troty,tr2rpy,rpy2tr\n",
    "from nao_defines import *\n",
    "base_dir =os.path.abspath(os.path.curdir)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 求解 AX=XB\n",
    "\n",
    "\\begin{align*}\n",
    "&\\begin{cases}\n",
    " A_1 T = T B_1 \\\\ \n",
    " A_2 T = T B_2 \\\\\n",
    "\\end{cases} \\\\ \n",
    "\\Rightarrow &\\begin{cases}\n",
    " R_{a1} R = R R_{b1} \\\\ \n",
    " R_{a1} P + P_{a1} = R P_{b1} + P \\\\\n",
    " R_{a2} R = R R_{b2} \\\\\n",
    " R_{a2} P + P_{a2} = R P_{b2} + P \\\\\n",
    "\\end{cases} \\\\ \n",
    " &\\begin{cases}\n",
    " R_{a1} R = R R_{b1} \\\\ \n",
    " R_{a2} R = R R_{b2} \\\\\n",
    "\\end{cases} \\\\ \n",
    " \\Rightarrow &\\begin{cases}\n",
    " k_{a1} = R k_{b1} \\\\ \n",
    " k_{a2} = R k_{b2} \\\\\n",
    " (k_{a1} \\times k_{a2}) = R (k_{b1} \\times k_{b2}) \\\\\n",
    "\\end{cases} \\\\ \n",
    " \\Rightarrow & [k_{a1},k_{a2},k_{a1} \\times k_{a2}] = R [k_{b1},k_{b2},k_{b1} \\times k_{b2}] \\\\\n",
    " \\Rightarrow & R = [k_{a1},k_{a2},k_{a1} \\times k_{a2}][k_{b1},k_{b2},k_{b1} \\times k_{b2}]^{-1} \\\\\n",
    "  &\\begin{cases}\n",
    " R_{a1} P + P_{a1} = R P_{b1} + P \\\\\n",
    " R_{a2} P + P_{a2} = R P_{b2} + P \\\\\n",
    "\\end{cases}\\\\\n",
    " \\Rightarrow &\\begin{cases}\n",
    " (R_{a1} - I)P = R P_{b1} - P_{a1} \\\\\n",
    " (R_{a2} - I)P = R P_{b2} - P_{a2} \\\\\n",
    "\\end{cases}\\\\\n",
    "\\Rightarrow & \\begin{bmatrix}\n",
    "R_{a1}-I\\\\ \n",
    "R_{a2}-I\n",
    "\\end{bmatrix} P = \\begin{bmatrix}\n",
    "RP_{b1}-P_{a1}\\\\ \n",
    "RP_{b2}-P_{a2}\n",
    "\\end{bmatrix} \\\\\n",
    "\\Rightarrow & P = \\begin{bmatrix}\n",
    "R_{a1}-I\\\\ \n",
    "R_{a2}-I\n",
    "\\end{bmatrix}^{+}\\begin{bmatrix}\n",
    "RP_{b1}-P_{a1}\\\\ \n",
    "RP_{b2}-P_{a2}\n",
    "\\end{bmatrix}\n",
    "\\end{align*}"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def get_R(w):\n",
    "    R = cv2.Rodrigues(np.array(w))[0]\n",
    "    return mat(R)\n",
    "\n",
    "def get_w(R):\n",
    "    if R.shape == (4,4):\n",
    "        R = t2r(R)\n",
    "    w = cv2.Rodrigues(R)[0]\n",
    "    return mat(w/norm(w))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 求 R"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def norm_Mat(M):\n",
    "    #将矩阵M单位正交化\n",
    "    # 正交化\n",
    "    Eta = []\n",
    "    Beta = []\n",
    "    cols = M.shape[1]\n",
    "    for i in range(cols):\n",
    "        v = M[:,i]\n",
    "        beta = v\n",
    "        if Eta != []:\n",
    "            for eta in Eta:\n",
    "                beta = beta - (v.T*eta)[0,0]*eta\n",
    "        Beta.append(beta)\n",
    "        Eta.append(v/norm(v))\n",
    "    # 单位化\n",
    "    for index,beta in enumerate(Beta):\n",
    "        Beta[index] = beta/norm(beta)\n",
    "    # 生成输出矩阵\n",
    "    return array(Beta).reshape((-1,cols)).T"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def _get_better_R(R):\n",
    "    # 由于误差，得到的R并不是标准的单位正交矩阵，\n",
    "    # 通过先将其转换为角轴表示，在转换回来的方法可以得到,\n",
    "    # 不知为何 cv2.Rodrigues 得到的 R 并不是单位正交矩阵\n",
    "    #return cv2.Rodrigues(cv2.Rodrigues(R)[0])[0] \n",
    "    # 通过先将其转换为rpy，在转换回来\n",
    "    #return rpy2tr(tr2rpy(r2t(R)))[:3,:3]\n",
    "    return mat(norm_Mat(R))\n",
    "    #return R"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def solve_AX_XB_R(Ra1,Rb1,Ra2,Rb2):\n",
    "    ka1 = get_w(Ra1)\n",
    "    ka2 = get_w(Ra2)\n",
    "    ka1xka2 = np.cross(ka1,ka2,axis=0)\n",
    "    kb1 = get_w(Rb1)\n",
    "    kb2 = get_w(Rb2)\n",
    "    kb1xkb2 = np.cross(kb1,kb2,axis=0)\n",
    "\n",
    "    R = np.hstack((ka1,ka2,ka1xka2))*inv(np.hstack((kb1,kb2,kb1xkb2)))\n",
    "    return _get_better_R(R)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "最小二乘法求R\n",
    "\n",
    "\\begin{align*}\n",
    "& Ax = XB \\\\\n",
    "\\Rightarrow & \\vec{k}_a = R \\vec{k}_b \\\\\n",
    "\\Rightarrow & \\begin{bmatrix}\n",
    "r_{11} & r_{12} & r_{13} \\\\ \n",
    "r_{21} & r_{22} & r_{23} \\\\ \n",
    "r_{31} & r_{32} & r_{33}\n",
    "\\end{bmatrix} \\vec{k}_b = \\vec{k}_a \\\\\n",
    "\\Rightarrow & \\begin{bmatrix}\n",
    "\\vec{r}_1^T \\\\ \n",
    "\\vec{r}_2^T \\\\ \n",
    "\\vec{r}_3^T\n",
    "\\end{bmatrix} \\vec{k_b} = \\vec{k}_a \\\\\n",
    "\\Rightarrow & \\begin{bmatrix}\n",
    "\\vec{k}_b^T & 0 & 0 \\\\ \n",
    "0 & \\vec{k}_b^T & 0 \\\\ \n",
    "0 & 0 & \\vec{k}_b^T\n",
    "\\end{bmatrix} \\begin{bmatrix}\n",
    "\\vec{r}_1 \\\\ \n",
    "\\vec{r}_2 \\\\ \n",
    "\\vec{r}_3\n",
    "\\end{bmatrix} = \\vec{k}_a \\\\\n",
    "\\Rightarrow & \\begin{bmatrix}\n",
    "\\vec{k}_{b1}^T & 0 & 0 \\\\ \n",
    "0 & \\vec{k}_{b1}^T & 0 \\\\ \n",
    "0 & 0 & \\vec{k}_{b1}^T \\\\\n",
    "\\vdots & \\vdots & \\vdots \\\\\n",
    "\\vec{k}_{bn}^T & 0 & 0 \\\\ \n",
    "0 & \\vec{k}_{bn}^T & 0 \\\\ \n",
    "0 & 0 & \\vec{k}_{bn}^T \\\\\n",
    "\\end{bmatrix} \\begin{bmatrix}\n",
    "\\vec{r}_1 \\\\ \n",
    "\\vec{r}_2 \\\\ \n",
    "\\vec{r}_3\n",
    "\\end{bmatrix} = \\begin{bmatrix}\n",
    "\\vec{k}_{a1} \\\\ \n",
    "\\vdots \\\\ \n",
    "\\vec{k}_{an}\n",
    "\\end{bmatrix} \\\\\n",
    "\\Rightarrow & A \\begin{bmatrix}\n",
    "\\vec{r}_1 \\\\ \n",
    "\\vec{r}_2 \\\\ \n",
    "\\vec{r}_3\n",
    "\\end{bmatrix} = b \\\\\n",
    "\\Rightarrow & \\begin{bmatrix}\n",
    "\\vec{r}_1 \\\\ \n",
    "\\vec{r}_2 \\\\ \n",
    "\\vec{r}_3\n",
    "\\end{bmatrix} = A^+ b \\\\\n",
    "\\end{align*}\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "改进的最小二乘法求R, 因为误差的缘故，直接由最小二乘法得到的 R 并不能保证是单位正交矩阵，改进方法如下："
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def solve_AX_XB_R_List(RaL,RbL):\n",
    "    _A,_b = None,None\n",
    "    for Ra,Rb in zip(RaL,RbL):\n",
    "        ka = get_w(Ra)\n",
    "        kb = get_w(Rb)\n",
    "        A = mat(np.zeros((3,9)))\n",
    "        A[0,0:3] = kb.transpose()\n",
    "        A[1,3:6] = kb.transpose()\n",
    "        A[2,6:9] = kb.transpose()\n",
    "        b = ka\n",
    "        if _A is None:\n",
    "            _A = A\n",
    "            _B = b\n",
    "        else:\n",
    "            _A = np.vstack((_A,A))\n",
    "            _B = np.vstack((_B,b))\n",
    "    R = (pinv(_A)*_B).reshape((3,3))\n",
    "    return _get_better_R(R)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 求 P"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def solve_AX_XB(AL,BL):\n",
    "    assert len(AL) == len(BL)\n",
    "    num = len(AL)\n",
    "    # 求 R\n",
    "    if num <2:\n",
    "        raise ValueError\n",
    "    elif num == 2:\n",
    "        R = solve_AX_XB_R(AL[0],BL[0],AL[1],BL[1])\n",
    "    else:\n",
    "        R = solve_AX_XB_R_List(AL,BL)\n",
    "    \n",
    "    # 求P\n",
    "    _A,_b = None,None\n",
    "    I = mat(eye(3))\n",
    "    for A,B in zip(AL,BL):\n",
    "        Ra = t2r(A)\n",
    "        Pa = transl(A)\n",
    "        Pb = transl(B)\n",
    "        if _A is None:\n",
    "            _A = Ra - I\n",
    "            _b = R*Pb - Pa\n",
    "        else:\n",
    "            _A = np.vstack((_A,Ra-I))\n",
    "            _b = np.vstack((_b,R*Pb - Pa))\n",
    "    P = pinv(_A)*_b\n",
    "    T = transl(P)*r2t(R)\n",
    "    return T"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 验证"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def get_B_from_A_X(A,X):\n",
    "    B = inv(X)*A*X\n",
    "    return B"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "# 验证 R 的求解\n",
    "realR = get_R([-0.45,1.2,0.2])\n",
    "Ra1 = get_R([0.25,0.5,0.8])\n",
    "Rb1 = get_B_from_A_X(Ra1,realR)\n",
    "Ra2 = get_R([0.5,0.21,0.32])\n",
    "Rb2 = get_B_from_A_X(Ra2,realR)\n",
    "Ra3 = get_R([0.16,0.41,0.90])\n",
    "Rb3 = get_B_from_A_X(Ra3,realR)\n",
    "\n",
    "R1 = solve_AX_XB_R(Ra1,Rb1,Ra2,Rb2)\n",
    "R2 = solve_AX_XB_R_List([Ra1,Ra2,Ra3],[Rb1,Rb2,Rb3])\n",
    "\n",
    "norm(R1-realR),norm(R2-realR)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "np.testing.assert_almost_equal(R1-realR,0)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "# 验证 P 的求解\n",
    "realT = transl(0.2,0.3,0.5)*r2t(realR)\n",
    "Ta1 = transl(4.2,7.3,6.2)*r2t(Ra1)\n",
    "Tb1 = get_B_from_A_X(Ta1,realT)\n",
    "Ta2 = transl(0.6,1.5,15.2)*r2t(Ra2)\n",
    "Tb2 = get_B_from_A_X(Ta2,realT)\n",
    "Ta3 = transl(1.5,5.6,8.4)*r2t(Ra3)\n",
    "Tb3 = get_B_from_A_X(Ta3,realT)\n",
    "\n",
    "T1 = solve_AX_XB([Ta1,Ta2],[Tb1,Tb2])\n",
    "T2 = solve_AX_XB([Ta1,Ta2,Ta3],[Tb1,Tb2,Tb3])\n",
    "\n",
    "norm(T1-realT),norm(T2-realT)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 手眼标定"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 数据准备"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def my_unique(states):\n",
    "    # 移除重复的数据\n",
    "    out=[]\n",
    "    prv = {}\n",
    "    for state in states:\n",
    "        angles = str(state.getAngles(\"Head\"))\n",
    "        if angles in prv:\n",
    "            continue\n",
    "        else:\n",
    "            prv[angles] = True\n",
    "            out.append(state)\n",
    "    return out"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def init_nao(nao):\n",
    "    nao.top_state = array(my_unique(nao.get_states(\"cam\",top=True)))\n",
    "    nao.bottom_state = array(my_unique(nao.get_states(\"cam\",bottom=True)))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def init_Tf(nao):\n",
    "    ss = sql.Session()\n",
    "    for state in nao.states:\n",
    "        top_tf = state.findTr(\"top\")\n",
    "        bottom_tf = state.findTr(\"bottom\")\n",
    "        state.setTr(\"top\",top_tf)\n",
    "        state.setTr(\"bottom\",bottom_tf)\n",
    "        ss.add(state)\n",
    "    ss.commit()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "ss = sql.Session()\n",
    "red = sql.Nao.get_by_name(\"blue\")\n",
    "init_nao(red)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "#init_Tf(red) # 初始化标定板位姿矩阵"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 本实验中求解"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "\\begin{cases}\n",
    "R = [k_{a1},k_{a2},k_{a1} \\times k_{a2}][k_{b1},k_{b2},k_{b1} \\times k_{b2}]^{-1} \\\\\n",
    "P = \\begin{bmatrix}\n",
    "R_{a1}-I\\\\ \n",
    "R_{a2}-I\n",
    "\\end{bmatrix}^{+}\\begin{bmatrix}\n",
    "RP_{b1}-P_{a1}\\\\ \n",
    "RP_{b2}-P_{a2}\n",
    "\\end{bmatrix}\n",
    "\\end{cases}"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "def solve_Tec(states1,states2,position='top'):\n",
    "    AL = []\n",
    "    BL = []\n",
    "    nao = sql.Nao.get(states1[0].nao_id)\n",
    "    for state1,state2 in zip(states1,states2):\n",
    "        Tc1_0 = state1.getTr(position)\n",
    "        Tc2_0 = state2.getTr(position)\n",
    "        if Tc1_0 is None or Tc2_0 is None:\n",
    "            return solve_Tec(states1,states2,position='bottom') if position == 'top' else None\n",
    "        Tc2_c1 = Tc2_0*linalg.inv(Tc1_0)\n",
    "        B = np.mat(Tc2_c1)\n",
    "        TB_e1 = nao.kin.Head.fkine(state1.getAngles(\"Head\"))\n",
    "        TB_e2 = nao.kin.Head.fkine(state2.getAngles(\"Head\"))\n",
    "        Te2_e1 = linalg.inv(TB_e2)*TB_e1\n",
    "        A = np.mat(Te2_e1)\n",
    "        AL.append(A)\n",
    "        BL.append(B)\n",
    "    return solve_AX_XB(AL,BL)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "Matrix(solve_Tec(red.top_state[(1,5),],red.top_state[(10,17),]))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "Matrix(solve_Tec(red.top_state[0::3],red.top_state[1::7]))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "# 过滤函数,将不可解的组合过滤掉\n",
    "def filter_state(states):\n",
    "    out_state = []\n",
    "    for state1 in states:\n",
    "        for state2 in states:\n",
    "            angles1 = state1.getAngles(\"Head\")\n",
    "            angles2 = state2.getAngles(\"Head\")\n",
    "            if ((angles1[0] == angles2[0]) or\n",
    "             (angles1[1] == angles2[1])):\n",
    "                continue\n",
    "            else:\n",
    "                out_state.append([state1,state2])\n",
    "    return array(out_state)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "# 找使得最终位置方差最小的一组解\n",
    "out_state = filter_state(red.top_state)\n",
    "T_top = solve_Tec(out_state[:,0],out_state[:,1])\n",
    "Matrix(T_top)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "# 找使得最终位置方差最小的一组解\n",
    "out_state = filter_state(red.bottom_state)\n",
    "T_bottom = solve_Tec(out_state[:,0],out_state[:,1])\n",
    "Matrix(T_bottom)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def set_tool(nao,T_top,T_bottom):\n",
    "    ss = sql.Session()\n",
    "    nao = ss.query(sql.Nao).get(nao.id)\n",
    "    nao.top_cam.set_tool(nao.kin.Head.tool*T_top)\n",
    "    nao.bottom_cam.set_tool(nao.kin.Head.tool*T_bottom)\n",
    "    ss.add(nao)\n",
    "    ss.commit()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "#T_top = solve_Te_c(red.top_state[(1,5),],red.top_state[(10,17),])\n",
    "#T_top = solve_Tec(red.top_state[0::3],red.top_state[1::7])\n",
    "#T_bottom = solve_Te_c(red.bottom_state[(1,5),],red.bottom_state[(10,17),])\n",
    "#T_bottom = solve_Tec(red.bottom_state[0::3],red.bottom_state[1::7])\n",
    "set_tool(red,T_top,T_bottom)\n",
    "red.kin.load_camera_tool()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 误差分析"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "def get_xyzw(obj):\n",
    "    W = None\n",
    "    for T in obj:\n",
    "        w = tr2rpy(T)\n",
    "        if W is None:\n",
    "            W = w\n",
    "        else:\n",
    "            W = np.vstack((W,w))\n",
    "    x = obj[:,0,3]\n",
    "    y = obj[:,1,3]\n",
    "    z = obj[:,2,3]\n",
    "    wz = W[:,0]\n",
    "    wy = W[:,1]\n",
    "    wx = W[:,2]\n",
    "    wx = np.rad2deg(wx)\n",
    "    wy = np.rad2deg(wy)\n",
    "    wz = np.rad2deg(wz)\n",
    "    return x,y,z,wx,wy,wz"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def print_xyzw(obj):\n",
    "    x,y,z,wx,wy,wz = get_xyzw(obj)\n",
    "    for s,v in zip(['x','y','z','wx','wy','wz'],[x,y,z,wx,wy,wz]):\n",
    "        string = \"\\n\"\n",
    "        string += \"mean %s: %f \\n\" % (s,np.mean(v))\n",
    "        string += \"max %s: %f \\n\" % (s,np.max(v))\n",
    "        string += \"min %s: %f \\n\" % (s,np.min(v))\n",
    "        string += \"delta %s: %f \\n\" % (s,np.max(v) - np.min(v))\n",
    "        print string"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "def plot_T(obj,l='-',show = True):\n",
    "    t = np.arange(len(obj))\n",
    "    x,y,z,wx,wy,wz = get_xyzw(obj)\n",
    "        \n",
    "    plt.figure(1)\n",
    "    plt.subplot(311)\n",
    "    plt.plot(t,x,'r'+l)\n",
    "\n",
    "    plt.subplot(312)\n",
    "    plt.plot(t,y,'g'+l)\n",
    "    \n",
    "    plt.subplot(313)\n",
    "    plt.plot(t,z,'b'+l)\n",
    "    \n",
    "    plt.figure(2)\n",
    "    plt.subplot(311)\n",
    "    plt.plot(t,wx,'r'+l)\n",
    "\n",
    "    plt.subplot(312)\n",
    "    plt.plot(t,wy,'g'+l)\n",
    "    \n",
    "    plt.subplot(313)\n",
    "    plt.plot(t,wz,'b'+l)\n",
    "    \n",
    "    if show:\n",
    "        plt.show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "top_out_state = filter_state(red.top_state)\n",
    "bottom_out_state = filter_state(red.bottom_state)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### 仅使用最小二乘"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def _get_better_R(R):\n",
    "    return R"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "T_top = solve_Tec(top_out_state[:,0],top_out_state[:,1])\n",
    "T_bottom = solve_Tec(bottom_out_state[:,0],bottom_out_state[:,1])\n",
    "set_tool(red,T_top,T_bottom)\n",
    "red.kin.load_camera_tool()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "red_obj1 = np.array([ state.get_obj() for state in red.top_state])\n",
    "print_xyzw(red_obj1) # 使用 单位正交化\n",
    "plot_T(red_obj1)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "tmp_obj = red_obj1.copy()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### 使用单位正交化"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def _get_better_R(R):\n",
    "    return mat(norm_Mat(R))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "T_top = solve_Tec(top_out_state[:,0],top_out_state[:,1])\n",
    "T_bottom = solve_Tec(bottom_out_state[:,0],bottom_out_state[:,1])\n",
    "set_tool(red,T_top,T_bottom)\n",
    "red.kin.load_camera_tool()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "red_obj1 = np.array([ state.get_obj() for state in red.top_state])\n",
    "print_xyzw(red_obj1) # 使用 单位正交化\n",
    "plot_T(red_obj1)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "plot_T(red_obj1,'-',show=False)\n",
    "plot_T(tmp_obj,'-.')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "collapsed": false
   },
   "source": [
    "```\n",
    "T_top = solve_Tec(red.top_state[::3],red.top_state[1::7])\n",
    "T_bottom = solve_Tec(red.bottom_state[::3],red.bottom_state[1::7])\n",
    "set_tool(red,T_top,T_bottom)\n",
    "red.kin.load_camera_tool()\n",
    "red_obj2 = np.array([ state.get_obj() for state in red.top_state])\n",
    "print_xyzw(red_obj2) # 结果显示不如上一个好\n",
    "```"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### 使用 rpy2tr"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def _get_better_R(R):\n",
    "    return rpy2tr(tr2rpy(r2t(R)))[:3,:3]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "T_top = solve_Tec(top_out_state[:,0],top_out_state[:,1])\n",
    "T_bottom = solve_Tec(bottom_out_state[:,0],bottom_out_state[:,1])\n",
    "set_tool(red,T_top,T_bottom)\n",
    "red.kin.load_camera_tool()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "red_obj1 = np.array([ state.get_obj() for state in red.top_state])\n",
    "print_xyzw(red_obj1) # 使用 单位正交化\n",
    "plot_T(red_obj1)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "plot_T(red_obj1,'-',show=False)\n",
    "plot_T(tmp_obj,'-.')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### 使用 cv2.Rodrigues"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def _get_better_R(R):\n",
    "    # 不知为何 cv2.Rodrigues 得到的 R 并不是单位正交矩阵\n",
    "    return cv2.Rodrigues(cv2.Rodrigues(R)[0])[0] "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "T_top = solve_Tec(top_out_state[:,0],top_out_state[:,1])\n",
    "T_bottom = solve_Tec(bottom_out_state[:,0],bottom_out_state[:,1])\n",
    "set_tool(red,T_top,T_bottom)\n",
    "red.kin.load_camera_tool()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "red_obj1 = np.array([ state.get_obj() for state in red.top_state])\n",
    "print_xyzw(red_obj1) # 使用 单位正交化\n",
    "plot_T(red_obj1)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### 使用 SVD"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def _get_better_R(R):\n",
    "    S,V,D = svd(R)\n",
    "    R = S*D\n",
    "    return R"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "T_top = solve_Tec(top_out_state[:,0],top_out_state[:,1])\n",
    "T_bottom = solve_Tec(bottom_out_state[:,0],bottom_out_state[:,1])\n",
    "set_tool(red,T_top,T_bottom)\n",
    "red.kin.load_camera_tool()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "red_obj1 = np.array([ state.get_obj() for state in red.top_state])\n",
    "print_xyzw(red_obj1) # 使用 单位正交化\n",
    "plot_T(red_obj1)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "plot_T(red_obj1,'-',show=False)\n",
    "plot_T(tmp_obj,'-.')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "collapsed": true
   },
   "source": [
    "## 误差补偿"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "误差存在规律,即存在系统误差,因为机器人不稳定,即使已经在每次采集图像前静待 10s.\n",
    "每次采集图片的代码如下:\n",
    "\n",
    "``` python\n",
    "for headYaw in np.linspace(minHeadYaw,maxHeadYaw,10):\n",
    "    for headPitch in np.linspace(minHeadPitch,maxHeadPitch,10):\n",
    "        setAngle([headYaw,headPitch])\n",
    "        time.sleep(10)\n",
    "        # 采集一次图片\n",
    "        nao.states.append(sql.State(\"cam\",nao,w,h,unit))\n",
    "```\n",
    "\n",
    "大规模采集图片过程中,每个相机共采集100张图片,但有的图像不合格,\n",
    "\n",
    "但观察上面的图片可以看到因为规律的图片采集得到了规律性的误差.\n",
    "\n",
    "考虑将系统误差视为线性误差和周期误差的综合,进行误差的补偿"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 2",
   "language": "python",
   "name": "python2"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 2
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython2",
   "version": "2.7.12"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 1
}
